#include "scara.h"
#include "omni.h"
#include <iostream>
#include <cmath>
#include <cstring>
#include <cstdio>   /* Standard input/output definitions =) */
#include <cstdlib>
#include "cv.h" 
#include "highgui.h"
#include "unistd.h"

using namespace std;

#define PTO_A 0
#define PTO_B 1
#define AZUL 0
#define VERDE 1
#define PILHA_DIR 0
#define PILHA_ESQ 1

// Makina de estados
#define INICIO			0
#define BLOCO_VERDE_NA_ESQ	1
#define BLOCO_AZUL_NA_DIR	2
#define BLOCO_AZUL_NA_ESQ	3
#define BLOCO_VERDE_NA_DIR	4

scara myScara(ARDUINO_SCARA); //cria um objeto scara e linka com o arduinoScara
omni myOmni(ARDUINO_OMNI);

bool pto_carga = PTO_B;
bool barco = VERDE;
bool bloco,pilha;
bool thread_main_iniciada = false;
int nextState = INICIO;

pthread_t t_main, t_timeOut;
  
void pegaBlocoNaPilha()
{
    cout << "Pilha da Direita(0) ou Esquerda(1)? " << endl;
    cin >> pilha;
    cout << "Bloco Azul(0) ou Verde(1)? " << endl;
    cin >> bloco;
  
    if( pilha == PILHA_DIR)
    {
      myScara.scaraGoTo(24.5,-3,-96); // lugar de pegar o bloco
      myScara.pegaBloco();
      myScara.scaraGoTo(24,-7,-96);
      myScara.scaraGoTo(24,-7,-61);
      myScara.scaraGoTo(19,-15.5,-44);
      myScara.scaraGoTo(2.5,-16.5,-95);
    }
    else
    {
      myScara.scaraGoTo(23.5,3.5,-93); // a calibrar
      myScara.pegaBloco();
      myOmni.alinhaIR(FRENTE,INTERNO);
      myScara.scaraGoTo(20,4.5,-2); 
      myScara.scaraGoTo(17,-10,-2);
      myScara.scaraGoTo(17,-15.5,-2);
      myScara.scaraGoTo(17,-15.5,-44);
      myScara.scaraGoTo(2.5,-16.5,-95);
    }
}


void deixaBlocoNoBarco( )
{
      myScara.scaraGoTo(19,-15.5,-44);
      myScara.scaraGoTo(19,0,0);
      myScara.deixaBloco();
}


void* thread_main(void*)
{
  int i = 0;
  
  cout << "Esperando Start..." << endl;
  
// While( arduino nao der start ) { espera... }
  myScara.esperaStart();
  
  cout << "START DADO!" << endl;
  thread_main_iniciada = true; // agora a thread_timeOut comeca a trabalhar...
  
  while( true )
  {
    switch( nextState )
    {
      case INICIO:	myOmni.go(FRENTE,740);

			myOmni.alinhaIR(FRENTE,EXTERNO);

			myOmni.go(FRENTE,130); 

			myOmni.alinhaIR(MEIO_ESQ,EXTERNO);
			myOmni.go(DIR,75);
			
			//--------------SCARA IDENTIFICA BLOCOS
			// escolhe a cor que apresentar menor desvio padrao
			
			//--------------SCARA PEGA O BLOCO
			//while(1)
			pegaBlocoNaPilha();
			 
			myOmni.go(TRAS,60);
			
			if(bloco == VERDE) 
			{
			  myOmni.go(ESQ,420);
			  myOmni.alinhaIR(FRENTE,INTERNO);
			  myOmni.go(FRENTE,115);
			  myOmni.alinhaIR(MEIO_ESQ,EXTERNO);
			  myOmni.go(DIR,75);
			  nextState = BLOCO_VERDE_NA_ESQ;
			}
			else
			{
			  myOmni.go(DIR,420);
			  myOmni.alinhaIR(FRENTE,INTERNO);
			  myOmni.go(FRENTE,115);
			  myOmni.alinhaIR(MEIO_ESQ,EXTERNO);
			  myOmni.go(DIR,75);
			  nextState = BLOCO_AZUL_NA_DIR;
			}
	
      break;
      
      case BLOCO_VERDE_NA_ESQ:	myOmni.go(TRAS,100);
				myOmni.turn(CW,180);
				myOmni.alinhaIR(FRENTE,EXTERNO);
				
				// ----------------deixar o bloco
				//usleep(2000000);
				deixaBlocoNoBarco( );
				
				myOmni.go(TRAS,40);
				myOmni.turn(CCW,180);
				
				myOmni.alinhaIR(FRENTE,INTERNO);
				myOmni.go(FRENTE,100);
				myOmni.alinhaIR(MEIO_ESQ,EXTERNO);
				myOmni.go(DIR,75);
				
				// ----------------pega bloco
				pegaBlocoNaPilha();
				
				while(1)
				{ ; }
				
				if( bloco == VERDE )
				  nextState = BLOCO_VERDE_NA_ESQ;
				else
				  nextState = BLOCO_AZUL_NA_ESQ;
			      
      break;
   
      case BLOCO_AZUL_NA_DIR:	myOmni.go(TRAS,100);
				myOmni.turn(CW,180);
				myOmni.alinhaIR(FRENTE,EXTERNO);
				
				// deixar o bloco
				deixaBlocoNoBarco();
				
				myOmni.go(TRAS,50);
				myOmni.turn(CCW,180);
				
				myOmni.alinhaIR(FRENTE,INTERNO);
				myOmni.go(FRENTE,100);
				myOmni.alinhaIR(MEIO_ESQ,EXTERNO);
				myOmni.go(DIR,75);
				
				// ----------------pega bloco
				pegaBlocoNaPilha();
				
				if( bloco == AZUL )
				  nextState = BLOCO_AZUL_NA_DIR;
				else
				  nextState = BLOCO_VERDE_NA_DIR; 
	
      break;
      
      case BLOCO_AZUL_NA_ESQ:	myOmni.alinhaIR(FRENTE,INTERNO);
				myOmni.go(FRENTE,40);
				usleep(300000); // pra garantir
				myOmni.go(DIR,810);
				myOmni.alinhaIR(FRENTE,INTERNO);
				myOmni.go(FRENTE,115);
				myOmni.alinhaIR(MEIO_ESQ,EXTERNO);
				myOmni.go(DIR,75);
				
				nextState = BLOCO_AZUL_NA_DIR;
		    
      break;
      
      case BLOCO_VERDE_NA_DIR:	myOmni.alinhaIR(FRENTE,INTERNO);
				myOmni.go(FRENTE,100);
				myOmni.go(ESQ,810);
				myOmni.alinhaIR(FRENTE,INTERNO);
				myOmni.go(FRENTE,115);
				myOmni.alinhaIR(MEIO_ESQ,EXTERNO);
				myOmni.go(DIR,75);
				
				nextState = BLOCO_VERDE_NA_ESQ;
      break;
    
    }
  }
}






void* thread_timeOut(void* obj)
{
  
REINICIO:

  long tempoDecorrido = 0;
  struct timeval tNow;
  
  pthread_create(&t_main, 0,thread_main, NULL);
  
  while( thread_main_iniciada == false )
  { usleep(5000); } 
  
  usleep(2000000);
  
  while( true )
  {

    
    /*----------------------- ARDUINO DO OMNI---------------------------*/

    if( myOmni.le_msgEnviada()==1 ) //ha pacotes em transito
    {
      gettimeofday(&tNow, NULL);
      
      tempoDecorrido = tNow.tv_sec - (myOmni.le_t_envio()).tv_sec;
      tempoDecorrido = tempoDecorrido*1000000 + (tNow.tv_usec - (myOmni.le_t_envio()).tv_usec);
            
      if(tempoDecorrido > myOmni.le_timeOut())
      {
	cout << "ESTOUROU O TIME_OUT de " << (myOmni.le_timeOut()) <<"seg!! Hora de retransmitir" << endl;
	myOmni.set_timeOutEstourado(true);
	usleep(2000000);
      }
      
      if( myOmni.le_msgEnviada() == 0 )
	cout << "Tempo de resposta do arduino: " << (float)tempoDecorrido/1000000 << endl;
	
    }
    
    
   /*----------------------- ARDUINO DO SCARA---------------------------*/
   
    if( myScara.le_msgEnviada()==1 ) //ha pacotes em transito
    {
      gettimeofday(&tNow, NULL);
      
      tempoDecorrido = tNow.tv_sec - (myScara.le_t_envio()).tv_sec;
      tempoDecorrido = tempoDecorrido*1000000 + (tNow.tv_usec - (myScara.le_t_envio()).tv_usec);
            
      if(tempoDecorrido > myScara.le_timeOut())
      {
	cout << "ESTOUROU O TIME_OUT de " << (myScara.le_timeOut()) <<"seg!! Hora de retransmitir" << endl;
	myScara.set_timeOutEstourado(true);
	usleep(2000000);
      }
      
      usleep(10000);
      
      if( myScara.le_msgEnviada() == 0 )
	cout << "Tempo de resposta do arduino: " << (float)tempoDecorrido/1000000 << " seg" << endl;
	
    }
    
    if( myScara.le_serial() == 1 ) // REBOOT requerido pelo arduino!
    { 
      cout << "ARDUINO-SCARA DEU REBOOOOOT!" << endl;
      
      while( pthread_cancel(t_main) != 0 ) // cancela a thread_main
      { ; }
      
      myOmni.reset();
      
      thread_main_iniciada = false;
      goto REINICIO;
    }
  
  
  usleep(1000000); //thread executa a cada 1seg  
  }
}


int main(void)
{
//   pthread_create(&t_main, 0,thread_main, NULL);
  pthread_create(&t_timeOut, 0,thread_timeOut, NULL);
  
  //espera as threads terminarem...
//  pthread_join(t_main, NULL);
  pthread_join(t_timeOut, NULL);
        
return 0;
}

